#include "Colider.h"
#include <GL\glut.h>
#include "GameConstants.h"
#include "Engine.h"

#include <Windows.h>
#include <sstream>

const Vector3<float> Colider::Z_FORWARD = Vector3<float>(0,0,-1);

// construtors set up coliders with basic parimiters and the make the moment of inirta
Colider::Colider(std::vector<Triangle> shape, Vector3<float> location, float mass):
	_model(shape),
	_position(location),
	_mass(mass),
	_rotation(Quaternion::NO_ROTATION),
	_scale(Vector3<float>(1,1,1)),
	_angularVelocity(Quaternion::NO_ROTATION)
{
	makeMOI();
	TransformPoints();
}

Colider::Colider(std::vector<Triangle> shape, Vector3<float> location, float mass, Quaternion rotation):
	_model(shape),
	_position(location),
	_mass(mass),
	_rotation(rotation),
	_scale(Vector3<float>(1,1,1)),
	_angularVelocity(Quaternion::NO_ROTATION)
{
	makeMOI();
	TransformPoints();
}

Colider::Colider(std::vector<Triangle> shape, Vector3<float> location, float mass, Quaternion rotation, Vector3<float> scale):
_model(shape),
_position(location),
_mass(mass),
_rotation(rotation),
_scale(scale),
_angularVelocity(Quaternion::NO_ROTATION)
{
	makeMOI();
	TransformPoints();
}


Colider::~Colider(void)
{
}

// basic update function applys forces then changes postion and angle
void Colider::Update()
{
	ApplyForces();

	_position += _velocity * Engine::dt;
	_rotation *= _angularVelocity;

	ClearForces();
	TransformPoints();
}
// its a draw ya gl
void Colider::Draw(void)
{	
	glPushMatrix();
	
	glTranslatef(_position.X(), _position.Y(), _position.Z());
	glMultMatrixd(_rotation.asMatrix().GetRoto());
	glScalef(_scale.X(), _scale.Y(), _scale.Z());
	
	glBegin(GL_TRIANGLES); 
	glColor3f(_color.r, _color.g, _color.b);
	
	for(int i = 0; i < _model.size(); i++)
	{
		_model[i].Draw();
	}

	glEnd();

	glBegin(GL_LINES); 
	glColor3f(1,1,1);
	for(int i = 0; i < _model.size(); i++)
	{
		_model[i].Draw();
		glVertex3f(_model[i].Points[0].X(), _model[i].Points[0].Y(), _model[i].Points[0].Z());
	}
	glEnd();

	glPopMatrix();

	//BoundingBox(_transformed).Draw();
}

void Colider::ManualDraw()
{
	// this method draws the the points throug a manual transformations 
	glBegin(GL_TRIANGLES);
	glColor4f(_color.r,_color.g,_color.b,1);

	for(int i = 0; i < _transformed.size(); i++)
	{
		_transformed[i].Draw();
	}

	glEnd();

	glBegin(GL_LINES); 
	glColor3f(1,1,1);
	for(int i = 0; i < _model.size(); i++)
	{
		_transformed[i].Draw();
		glVertex3f(_transformed[i].Points[0].X(), _transformed[i].Points[0].Y(), _transformed[i].Points[0].Z());
	}
	glEnd();
}

void Colider::DrawNormals()
{
	glPushMatrix();
	
	glTranslatef(_position.X(), _position.Y(), _position.Z());
	glMultMatrixd(_rotation.asMatrix().GetRoto());
	glScalef(_scale.X(), _scale.Y(), _scale.Z());
	for(int i = 0; i < _model.size(); i++)
	{
		_model[i].DrawNormal();
	}

	glPopMatrix();
}

//////////////////////////////////  Colision Methods
// alow for both refrences or pointers to be called on
bool Colider::SATTest(Colider obj, Vector3<float>& myColide, Vector3<float>& objColide, Vector3<float>& colisionNormal, float& overLap)
{ return SATTest(&obj, myColide, objColide, colisionNormal, overLap); }

bool Colider::SATTest(Colider* obj, Vector3<float>& myColide, Vector3<float>& objColide, Vector3<float>& colisionNormal, float& overLap)
{
	overLap = FLT_MAX;
	// get vector from here to there
	Vector3<float> seperation = obj->_position - _position;

	// find the closest point on each object
	// obj close point
	double smallestDistance = FLT_MAX;
	Vector3<float> objClosestPoint = Vector3<float>::ZERO;
	for(int  i = 0; i < obj->_transformed.size(); i++)
	{
		for(int j = 0; j < 3; j++)
		{
			double sd =(obj->_transformed[i].Points[j] - _position).MagnitudeSquared();
			if( sd < smallestDistance)
			{
				smallestDistance = sd;
				objClosestPoint = obj->_transformed[i].Points[j];
			}
		}
	}
	// get my closeist point
	smallestDistance = FLT_MAX;
	Vector3<float> myClosestPoint = Vector3<float>::ZERO;
	for(int  i = 0; i < _transformed.size(); i++)
	{
		for(int j = 0; j < 3; j++)
		{
			double sd =(_transformed[i].Points[j] - obj->_position).MagnitudeSquared();
			if( sd < smallestDistance)
			{
				smallestDistance = sd;
				myClosestPoint = _transformed[i].Points[j];
			}
		}
	}

	// run STA using speration as a normal
		// get points relitive to this objects center
	myClosestPoint -= _position;
	objClosestPoint -= _position;

	Vector3<float> myPointOnLine = seperation * (myClosestPoint.Dot(seperation) / seperation.Dot(seperation));
	Vector3<float> objPointOnLine = seperation * (objClosestPoint.Dot(seperation) / seperation.Dot(seperation));

	myColide = myPointOnLine;
	objColide = objPointOnLine;
	
	double angle = Vector3<float>::AngleBetween(Z_FORWARD, seperation);
	Vector3<float> cross = Z_FORWARD.Cross(seperation);
	Quaternion rotation = Quaternion::NO_ROTATION;

	if(cross == Vector3<float>::ZERO)
	{
		// normal is along z axis
		if(seperation.Normal() == Z_FORWARD)
		{
			rotation = Quaternion::NO_ROTATION;
		}
		else
		{
			rotation = Quaternion::AxisAngle(Vector3<float>(0,1,0), PI); 
		}		
	}
	else
	{
		// normal rotation
		rotation =  Quaternion::AxisAngle(Z_FORWARD.Cross(seperation), angle);
	}

	// rotate the points onto the z axis
	Vector3<float> mcp = rotation * myPointOnLine;
	Vector3<float> ocp = rotation * objPointOnLine;
		// if STA says hit
	if(ocp.Z() < mcp.Z())
	{
		// this means there is not a hit
		return false;
	}
	
	myPointOnLine += _position;
	objPointOnLine += _position;

	return SATPoint(objPointOnLine, colisionNormal, overLap);

}

// pass in a point will return true if point is inside of colider
bool Colider::SATPoint(Vector3<float> point, Vector3<float>& colisionNormal, float& overLap)
{
	Vector3<float> seperation = point - _position;
	Quaternion rotation = Quaternion::NO_ROTATION;
	for(int i = 0; i < _transformed.size(); i++)
	{
		// if angle between normal and sepration is  > 90 digress
		double angle = Vector3<float>::AngleBetween(_transformed[i].Normal, seperation);
		if(angle > 1.571f && angle < -1.571f) //angle > 1.571f && angle < -1.571f
		{
			// dont run the check assume hit
		}
		else
		{
			// do sta on normal and closest point
			angle = Vector3<float>::AngleBetween(Z_FORWARD, _transformed[i].Normal);
			Vector3<float> cross = Z_FORWARD.Cross(_transformed[i].Normal);
			if(cross == Vector3<float>::ZERO)
			{
				// normal is along z axis
				if(_transformed[i].Normal == Z_FORWARD)
				{
					rotation = Quaternion::NO_ROTATION;
				}
				else
				{
					rotation = Quaternion::AxisAngle(Vector3<float>(0,1,0), PI); 
				}
				
			}
			else
			{
				// normal rotation
				rotation = Quaternion::AxisAngle(cross, angle);
			}
			
			// point on the polygon rotated
			Vector3<float> mcp = rotation * (_transformed[i].Points[0] - _position);
			// point
			Vector3<float> rotatedPoint = rotation * seperation;

			if(rotatedPoint.Z() < mcp.Z())
			{
				return false;
			}
			else
			{
				if(abs(mcp.Z() - rotatedPoint.Z()) < overLap)
				{
					overLap = abs(mcp.Z() - rotatedPoint.Z());
					colisionNormal = _transformed[i].Normal;
				}
			}
			// else keep checking
		}
	}
	// if within all axis return true
	return true;
}


void Colider::CalulateVelocitesAfterColision(Vector3<float>& v1, double m1,
												Vector3<float>& v2, double m2,
												double CoefficientOfRestitution)
{
		//Non Elastic Colision   (elastic if Cr = 1)
		/*
			
				Cr*m2(vxCir - vxThis) + m1*vxThis + m2*vxCir
		vf1 =	--------------------------------------------
									m1 + m2
									   
				Cr*m1(vxThis - vxCir) + m1*vxThis + m2*vxCir
		vf2 =	--------------------------------------------
									m1 + m2
		*/
		double massSum = m1 + m2;
		double momentumSum = (m1 * v1.Z()) + (m2 * v2.Z());
			
		float vf1 = ((CoefficientOfRestitution * m2 * (v2.Z() - v1.Z())) + momentumSum)/ massSum; 
		float vf2 = ((CoefficientOfRestitution * m1 * (v1.Z() - v2.Z())) + momentumSum)/ massSum;
		v1.setZ(vf1);
		v2.setZ(vf2);
}

 void Colider::ForceColide(Colider* box1, Vector3<float> b1Hit,
							Colider* box2, Vector3<float> b2Hit,
							Vector3<float> ColisonNormal, float overLap)
 {
		//move the coliders so they are not touching 
		box1->Translate(ColisonNormal * (-overLap / 2));
		box2->Translate(ColisonNormal * (overLap / 2));
		// get the rotatoin for the velocites
		Quaternion rotation = Quaternion::NO_ROTATION;
		Vector3<float> cross = Z_FORWARD.Cross(ColisonNormal);
		if(cross == Vector3<float>::ZERO)
		{
			// normal is along z axis
			if(ColisonNormal == Z_FORWARD)
			{
				rotation = Quaternion::NO_ROTATION;
			}
			else
			{
				rotation = Quaternion::AxisAngle(Vector3<float>(0,1,0), PI); 
			}	
		}
		else
		{
			double angle = Vector3<float>::AngleBetween(Z_FORWARD, ColisonNormal);
			// normal rotation
			rotation = Quaternion::AxisAngle(cross, angle);
		}

		Vector3<float> v1 = rotation * box1->_velocity;
		Vector3<float> v1f = v1;
		Vector3<float> v2 = rotation * box2->_velocity;
		Vector3<float> v2f = v2;
		// calulate the final velocites 
		CalulateVelocitesAfterColision(v1f, box1->_mass, v2f, box2->_mass, 1);
		Vector3<float> force1(0,0,box2->_mass * (v1f.Z() - v1.Z()));
		Vector3<float> force2(0,0,box1->_mass * (v2f.Z() - v2.Z()));

		MyMatrix roto = rotation.asMatrix().Transpose();

		// rotate forces back
		MyMatrix f1(1,4);
		f1.at(0,0) = force1.X();
		f1.at(0,1) = force1.Y();
		f1.at(0,2) = force1.Z();
		f1.at(0,3) = 1;

		f1 = roto * f1;

		MyMatrix f2(1,4);
		f2.at(0,0) = force2.X();
		f2.at(0,1) = force2.Y();
		f2.at(0,2) = force2.Z();
		f2.at(0,3) = 1;

		f2 = roto * f2;
		// apply the forces
		box1->AddForce(Vector3<float>(f1.at(0,0),f1.at(0,1),f1.at(0,2)), b1Hit);
		box2->AddForce(Vector3<float>(f2.at(0,0),f2.at(0,1),f2.at(0,2)), b2Hit);
 }

/////////////////  end Colisions methods


// translate the camara for a current distance
void Colider::Translate(Vector3<float> distance)
{
	_position += distance;
}
// moves the camra to a postion in 3d sapce (a seter for the postion)
void Colider::TranslateTo(Vector3<float> destination)
{
	_position = destination;
}

// 
void Colider::AddForce(Vector3<float> force, Vector3<float> radious)
	{
	Vector3<float> torque = radious.Cross(force); // this gets the direction of the torque
	if(radious == Vector3<float>::ZERO || torque == Vector3<float>::ZERO)
	{
		_liniarForces += force;
		return;
	}
	

	// angular changes
	torque = torque.Normal();
	double angle = radious.AngleBetween(force);
	// give torque its magnitude
	double si = sin(angle);
	torque *= (force.Magnitude() * radious.Magnitude() * sin(angle));;
	_torque += torque;
	// do the liniar force part // ignored for this project to simplify
	_liniarForces += force.Normal() * (force.Magnitude() * cos(angle));

	
}

// takes the forces acumplated and adds them to the proper places in the object
void Colider::ApplyForces()
{
	// calulate angular forces
	Vector3<float> moi = MOI();
	Vector3<float> alpha(_torque.X()/moi.X(), _torque.Y()/moi.Y(), _torque.Z()/moi.Z());

	_angularMomentum += alpha * 0.1;
	_angularMomentum *= GameConstants::ROTATIONAL_DAMPINING;

	// get the radians per second
	if(_angularMomentum == Vector3<float>::ZERO)
	{
		_angularVelocity = Quaternion::NO_ROTATION;
	}
	else
	{
		float rads = _angularMomentum.Magnitude() * Engine::dt * Engine::dt;
		_angularVelocity = Quaternion::AxisAngle(_angularMomentum, rads);
	}
	
	// liinar velocites
	_velocity += _liniarForces * (1/_mass) * Engine::dt;

	_velocity *= GameConstants::LINIAR_DAMPINING;
}

void Colider::ClearForces()
{
	_liniarForces = Vector3<float>::ZERO;
	_torque = Vector3<float>::ZERO;
}

// give the point as if it were a part of the model and being transformed
Vector3<float> Colider::PointOnShape(Vector3<float> start)
{
	// create the matrix 
	MyMatrix transform = _rotation.asMatrix();
	transform *= SkewMatrix();
	transform *= ScaleMatrix();

	MyMatrix point(1,4);
	point.at(0,0) = start.X();
	point.at(0,1) = start.Y();
	point.at(0,2) = start.Z();
	point.at(0,3) = 1;

	point = transform * point;

	return Vector3<float>(point.at(0,0), point.at(0,1), point.at(0,2));
}

Vector3<float> Colider::PointOnModel(Vector3<float> start)
{
	MyMatrix inverseScale(4,4);
	inverseScale.at(0,0) = 1/_scale.X();
	inverseScale.at(1,1) = 1/_scale.Y();
	inverseScale.at(2,2) = 1/_scale.Z();
	inverseScale.at(3,3) = 1;
	MyMatrix transform = inverseScale;

	MyMatrix inverseSkew(4,4);
	inverseSkew.at(0,0) = 1; // ones so no scale
	inverseSkew.at(1,1) = 1;
	inverseSkew.at(2,2) = 1;
	inverseSkew.at(3,3) = 1;
	// skew values
	inverseSkew.at(1,0) = -_skew.Y();
	inverseSkew.at(2,0) = -_skew.Z();
	inverseSkew.at(0,1) = -_skew.X();
	inverseSkew.at(3,1) = -_skew.Z();
	inverseSkew.at(0,2) = -_skew.X();
	inverseSkew.at(1,2) = -_skew.Y();
	transform *= inverseSkew;

	transform *= _rotation.asMatrix().Transpose();
	
	// take the point
	MyMatrix point(1,4);
	point.at(0,0) = start.X();
	point.at(0,1) = start.Y();
	point.at(0,2) = start.Z();
	point.at(0,3) = 1;

	point = transform * point;

	return Vector3<float>(point.at(0,0), point.at(0,1), point.at(0,2));
}

// returns the transformed model 
std::vector<Triangle> Colider::TransformedModel()
{
	return _transformed;
}

// makes the indivigural Moment of Inirtas for dimention
void Colider::makeMOI()
{
	float smallX = FLT_MAX;
	float largeX = FLT_MIN;
	float smallY = FLT_MAX;
	float largeY = FLT_MIN;
	float smallZ = FLT_MAX;
	float largeZ = FLT_MIN;

	for(int i = 0; i < _model.size(); i++)
	{
		for(int j = 0; j < 3; j++)
		{
			if(_model[i].Points[j].X() > largeX)
			{ largeX = _model[i].Points[j].X(); }
			else if(_model[i].Points[j].X() < smallX)
			{ smallX = _model[i].Points[j].X(); }

			if(_model[i].Points[j].Y() > largeY)
			{ largeY = _model[i].Points[j].Y(); }
			else if(_model[i].Points[j].Y() < smallY)
			{ smallY = _model[i].Points[j].Y(); }

			if(_model[i].Points[j].Z() > largeZ)
			{ largeZ = _model[i].Points[j].Z(); }
			else if(_model[i].Points[j].Z() < smallZ)
			{ smallZ = _model[i].Points[j].Z(); }
		}
	}

	Vector3<float> distances(largeX - smallX, largeY - smallY, largeZ - smallZ);

	_momentOfInirta.setX( (_mass * (distances.Y()*distances.Y() + distances.Z()*distances.Z())) / 12 );
	_momentOfInirta.setY( (_mass * (distances.X()*distances.X() + distances.Z()*distances.Z())) / 12 );
	_momentOfInirta.setZ( (_mass * (distances.X()*distances.X() + distances.Y()*distances.Y())) / 12 );
}

BoundingBox Colider::BBox()
{
	return BoundingBox(_transformed);
}

Vector3<float> Colider::MOI()
{
	return Vector3<float>(_momentOfInirta.X() * _scale.X(), _momentOfInirta.Y() * _scale.Y(), _momentOfInirta.Z() * _scale.Z());
}

Vector3<float>& Colider::Velocity(void){ return _velocity; }

Vector3<float>& Colider::Scale(void){	return _scale; }

Vector3<float>& Colider::Skew(void) {	return _skew; }

Quaternion& Colider::Rotation(void) { return _rotation; }

Vector3<float> Colider::Position(void)
{
	return _position;
}

// adds it a rotatoin
void Colider::Rotate(Quaternion amount)
{
	_rotation = _rotation * amount;
}

void Colider::RotateGlobal(Quaternion amount)
{
	_rotation = amount * _rotation;
}

// takes the basic points that represents the model and then traslate them into new points
void Colider::TransformPoints()
{
	// clear the points from the buffer
	_transformed.clear();
	// some matrixs to be used
	MyMatrix point(1,4);
	MyMatrix transform(4,4); // final matrix
	MyMatrix movedPoint(1,4);

	// create the transformation matrix
	transform = TranslateMatrix();
	transform *= _rotation.asMatrix();
	transform *= SkewMatrix();
	transform *= ScaleMatrix();

	//_rotation.asMatrix().ToString();
	//transform.ToString();
	for(int i = 0; i < _model.size(); i++)
	{
		_transformed.push_back(_model[i].Transformed(transform));
	} // do it for all points
}

// pull out the matrix creation to make it cleaner
// ther is no rotation because that is stored in the quaternion
MyMatrix Colider::ScaleMatrix()
{
	MyMatrix transform(4,4);
	// set up the transform 
	transform.at(0,0) = _scale.X();
	transform.at(1,1) = _scale.Y();
	transform.at(2,2) = _scale.Z();
	transform.at(3,3) = 1;

	return transform;
}

MyMatrix Colider::SkewMatrix()
{
	MyMatrix transform(4,4);

	transform.at(0,0) = 1; // ones so no scale
	transform.at(1,1) = 1;
	transform.at(2,2) = 1;
	transform.at(3,3) = 1;
	// skew values
	transform.at(1,0) = _skew.Y();
	transform.at(2,0) = _skew.Z();
	transform.at(0,1) = _skew.X();
	transform.at(3,1) = _skew.Z();
	transform.at(0,2) = _skew.X();
	transform.at(1,2) = _skew.Y();

	return transform;
}

MyMatrix Colider::TranslateMatrix()
{
	MyMatrix transform(4,4);

	////////// translate useing matrix
	transform.at(0,0) = 1; // ones so no scale
	transform.at(1,1) = 1;
	transform.at(2,2) = 1;
	transform.at(3,3) = 1;
	// matrix movemnt
	transform.at(3,0) = _position.X();
	transform.at(3,1) = _position.Y();
	transform.at(3,2) = _position.Z();

	return transform;
}